# Untitled - By: 11401 - Sun Jul 13 2025
import sensor,time,image,display
from pyb import Servo,UART,LED
#图像初始化
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.B320X320)
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
clock = time.clock()

#舵机初始化
Servo_1=Servo(1)
Servo_2=Servo(2)

#串口初始化
usart=UART(3,115200)

#灯初始化
LED_RED=LED(1)
LED_GREEN=LED(2)
LED_BULE=LED(3)

time.sleep(1)

red_color=(6, 70, -75, 102, 82, 23)
bule_color=(10, 73, 17, -44, -12, -60)
need_color=(10, 73, 17, -44, -12, -60)

roi = [0,0,320,240]

flag=0
#夹爪闭合+发送
def Claw_closure():
    Servo_1.angle(60,200)
    Servo_2.angle(60,200)
    sead=bytearray([0xFF,0xFE,0X12])
    usart.write(sead)

#夹爪张开
def opened_Claw():
    Servo_1.angle(45,200)
    Servo_2.angle(45,200)

#串口发送数据包
def uart_tack(task_id,data):
    sead=bytearray([0x55,0x43,task_id,data,0x11])
    usart.write(sead)


#读取串口
def read_usart():
    if usart.any() :
        rx=usart.readline().decode()
        print(rx)
        rx_bueff=list(rx)
        print(rx_bueff)
        print(rx_bueff[0])
        if rx_bueff[0] == 'A' and rx_bueff[1] == 'T':
            if rx_bueff[2] == 'B' :
                need_color=bule_color
                print(need_color)
            if rx_bueff[2] == 'R' :
                need_color=red_color
            if rx_bueff[2] == 'C':
                Claw_closure()
            if rx_bueff[2] == 'O':
                opened_Claw()
            if rx_bueff[3] == 'x':
                flag=1

Servo_1.angle(-80)
Servo_2.angle(-80)

while True:
    read_usart()
    clock.tick()
    imag = sensor.snapshot()
    blobs=imag.find_blobs([need_color],\
                        threshold = 3500, \
                        pixels_threshold=500,\
                        x_stride=2,\
                        y_stride=2,\
                        merge_cb =True)
    for blob in blobs:
        imag.draw_cross((blob.cx(),\
                        blob.cy()),\
                        color=(255,0,0),\
                        sixe=5,\
                        thickness=2)
        imag.draw_rectangle(blob.rect(),color=(0,255,0))
        if  flag==1 :
            if blob.cx() > 160 :
                Claw_closure()
    print(clock.fps())
